from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():

    # 启动Gazebo仿真环境
    gazebo_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('luck_gazebo'),
                'launch',
                'gazebo.launch.py'
            ])
        ])
    )

    # 启动MoveIt2
    moveit_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('luck_robot'), # 你的MoveIt2配置包名
                'launch',
                'demo.launch.py' # 使用demo.launch.py，它会启动MoveGroup和RViz
            ])
        ])
    )

    return LaunchDescription([
        gazebo_launch,
        moveit_launch,
    ])
